#ifndef ALGO_ASTAR_PRIVATE_H
#define ALGO_ASTAR_PRIVATE_H

#include "AlgoAStar.h"
#include "algohub_common_type.h"
#include <vector>
#include <memory>
#include <random>

namespace algo
{

// Forward declaration
class GridMap;

// Represents a node in the A* search space
struct Node {
    int x, y;
    double g_cost = 0.0;
    double h_cost = 0.0;
    Node* parent = nullptr;

    Node(int x, int y) : x(x), y(y) {}

    double getFCost() const { return g_cost + h_cost; }

    struct Compare {
        bool operator()(const Node* a, const Node* b) const {
            return a->getFCost() > b->getFCost();
        }
    };
};

// Private implementation class for AlgoAStar
class AlgoAStarPrivate
{
public:
    AlgoAStarPrivate();
    ~AlgoAStarPrivate();

    int exec(const algobase::TaskConfig& taskConfig, algobase::Paths& paths);

    // Setter for terrain data (as we can't modify TaskConfig)
    void setTerrainData(const std::vector<std::vector<double>>& terrain);

private:
    // --- Main Phases ---
    bool preprocess(const algobase::TaskConfig& taskConfig);
    
    // --- K-Shortest Path and Combination Logic ---
    std::vector<algobase::Path> findKShortestPaths(const std::pair<int, int>& start_grid, const std::pair<int, int>& end_grid, int K);
    algobase::Paths combinePaths(const std::vector<std::vector<algobase::Path>>& all_segment_paths);
    
    // --- Underlying A* Search Engine ---
    std::vector<Node*> findShortestPath(const std::pair<int, int>& start_grid, const std::pair<int, int>& end_grid, const std::map<std::pair<int, int>, double>& extra_costs);

    // --- Postprocessing and Helpers ---
    algobase::Path postprocessPath(const std::vector<Node*>& node_path);
    std::pair<int, int> findRepresentativeNodeForZone(const algobase::Polygon& polygon);
    double calculateHeuristic(int x1, int y1, int x2, int y2);
    double calculateCost(const Node* from, const Node* to);
    double getDepthFromTerrain(const algobase::Coord& coord) const;
    bool isInside(const algobase::Coord& point, const algobase::Polygon& polygon) const;
    double calculateGeographic3DDistance(const algobase::CoordZ& p1, const algobase::CoordZ& p2) const;


    std::unique_ptr<GridMap> grid_map_;
    std::vector<std::vector<double>> terrain_data_;
};

// GridMap class definition
class GridMap {
public:
    struct Cell {
        double depth = 0.0;
        bool is_walkable = true;
        double cost_multiplier = 1.0;
    };

    GridMap(double min_lon, double max_lon, double min_lat, double max_lat, double resolution);

    std::pair<int, int> worldToGrid(const algobase::Coord& coord) const;
    algobase::Coord gridToWorld(int x, int y) const;

    Cell& at(int x, int y);
    const Cell& at(int x, int y) const;

    int getWidth() const { return width_; }
    int getHeight() const { return height_; }
    void resetMultipliers() {
        for (auto& cell : cells_) {
            cell.cost_multiplier = 1.0;
        }
    }

private:
    double min_lon_, max_lon_, min_lat_, max_lat_;
    double lon_span_, lat_span_;
    double resolution_;
    int width_;
    int height_;
    std::vector<Cell> cells_;
};

} // namespace algo

#endif // ALGO_ASTAR_PRIVATE_H